#!/usr/bin/env python
#-*- coding: utf-8 -*-

import numpy as np
import math
import scipy
import matplotlib.pyplot as plt
import sys

import pat.vehicles.fixed_wing.dynamic_model_python_basic as dm
import pat.vehicles.fixed_wing.control_3d as ctl
import pat.utils as pu

# get configuration files from command line arguments
if len(sys.argv) > 1:
    filenames = sys.argv[1:]
else:
    filenames = ["../config/Rcam.xml"]

# load parameters
dms = [dm.DynamicModel(f) for f in filenames]
for d in dms: print d.param()

# finds trim conditions
va=70; gamma=pu.rad_of_deg(0.)
trims = [ctl.get_trim_cst_path(va, gamma, d.p, debug=True) for d in dms]

# run simulations
time = np.arange(0., 10., 0.01)
X = np.zeros((time.size,dm.sv_size))

def run_sim(time, du, du_axis, title):
    figure = None
    for k in range(0, len(dms)):
        Xe, Ue = trims[k]
        U = Ue*np.ones((len(time), dms[k].input_nb()))
        U[:,getattr(dms[k], du_axis)()] += du
        X[0] = dms[k].reset(Xe)
        for i in range(1,time.size):
            X[i,:] = dms[k].run(time[i] - time[i-1], U[i-1])
        figure = dm.plot_trajectory(time, X, figure, window_title="{:s} ({:s})".format(title, filenames))
    plt.legend(filenames)


# runs a simulation with throttle steps
du = 0.3*scipy.signal.square(time+math.pi/2.)
run_sim(time, du, "iv_dth", "Throttle steps")

# runs a simulation with ailerons steps
du = pu.rad_of_deg(15)*scipy.signal.square(time+math.pi/2.)
run_sim(time, du, "iv_da", "Ailerons steps")

# runs a simulation with rudder steps
du = pu.rad_of_deg(15)*scipy.signal.square(time+math.pi/2.)
run_sim(time, du, "iv_dr", "Rudder steps")

# runs a simulation with elevator steps
du = pu.rad_of_deg(5)*scipy.signal.square(time+math.pi/2.)
run_sim(time, du, "iv_de", "Elevator steps")
plt.savefig("../doc/images/example_03_input_step_4.png", dpi=80)

plt.show()
